COMMENT ⊗   VALID 00006 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	 POINTY and AL display routines and initialization
C00005 00003		initialization routine	UPDINIT
C00009 00004		routine to print out joint value PRTW
C00014 00005		routine to read joint angles every 100 msec DOWHER
C00016 00006	LOCAL STORAGE AREA
C00020 ENDMK
C⊗;
COMMENT ⊗ POINTY and AL display routines and initialization
	⊗;
TTTTRANS==1
NNCYCLES==50.

DATA
TRNADR:	.BLKW	60
CODE

;DEVICE MECHANISM BITS

YELARM== 1	;MECHANISM BITS: YELLOW ARM, NOT INCLUDING HAND
YELHND== 2 	;YELLOW HAND
BLUARM== 4	;BLUE ARM, NOT INCLUDING HAND
BLUHND== 10	;BLUE HAND
BTABOFF==14.	;OFFSET FOR POINTERS INTO TABLES

COMMENT ⊗
	The following are the character codes for the VT05


	HOME POSITION		35	..use with fillers, e.g. 10
		Move curson to top line, first character pos
	ERASE LINE		36
		... to the end of the line from the current cursor pos
	ERASE SCREEN		37	.. use with fillers
		... to end of screen from current pos
	LINE FEED		12
		... if on last line, cause scroll
	BELL			 7
	BACKSPACE		10??	..(not delete)
	CR			15
	C(→) cursor right	30
	C(←) cursor left	10
	C(↑) curson up		32
	C(↓) cursor down(one line)13
	CAD			16	.. USE FILLER
		.. direct cursor addressing
		.. next two words(bytes?) represent
			y-addres or line location
			x-address of char location
				(except that octal 40 - 1 must be added
				if you assume that lines begin 
				at 1 instead of 0)
⊗;


HOME==35
EOL==36
EOS==37
TAB==11
CR==15
LF==12
BELL==7
BS==10
CRR==30
CRL==10
CRU==32
CRD==13
CAD==16
COFF==37
CFILL==10		;Used as filler because 0 will be treated as end of string
COMMA==',
ALHOMY==COFF+14		;Ycoord code for beginning of AL display section
JTHOMY==COFF+7		;Ycoord code for beginning of joint display line
ADHOMY==COFF+10		;Ycoord code for beginning of A-D reading display line
EUHOMY==COFF+11		;Ycoord code for beginning of euler angle display line

JTHOMX==COFF+17
EUHOMX==COFF+17
ALHOMX==COFF+1
ADHOMX==COFF+17
;	initialization routine	UPDINIT
;	
;	This routine initializes the updating by clearing the
;VT05 screen, and typing the header information and the titles
;and then starts up two jobs PRWT and DOWHER which continously print
;out and update the joint angle information
;	All registers are saved
;
UPDINIT: PUSH	<R0,R1,R2,R3,R4,R5>
	EVWAIT	CSLEVT
	MOV	#CLRSCR,R0	;GO TO TOP OF PAGE ON VT05 and clear VT05
	JSR	PC,PTYPSTR
	EVSIG	CSLEVT
	CLR	FRMDDT		;to clear from run to run
	CLR	DSPOK		;turn on display joint angles and trans angles
	CLR	UPDOK		;makes sure we will start keep doing where
	JSR	PC,DSPINIT	;INITIALISE THE DISPLAY
	SCHEDU	#DWHRPD,#DOWHER,#USRDM,#2	;SCHEDULE JOB FOR READING ARM CONTIN
	SCHEDU	#PRTWPD,#PRTW,#USRDM,#4	;SCHEDULE JOB FOR PRINTING OUT DISPLAY
	POP	<R5,R4,R3,R2,R1,R0>
	RTS	PC

DSPINIT:PUSH	<R2,R3,R4,R5>	; we will need this later
	MOV	#8.,R0		;SET FORMAT TO PRINT OUT COLUMNS WITH WIDTH = 8
	MOV	#2,R1		;AND DIGITS = 2
	JSR	PC,FORMAT
	MOV	#NNCYCLES,NCYCLES	; set ncycles=whatever it is
	CMP	VT05DSP,#1	;blue only?
	BEQ	1$
	MOV	#200$,R5	;yarm
	CLR	R4
	BR	11$
1$:	MOV	#100$,R5
	MOV	#14.,R4
11$:	MOV	LUSTPPTR,R2	;starting address of upper stop
	ADD	R4,R2
	MOV	#ULINE,R0
	MOV	#7,R3
9$:	LDF	@(R2)+,AC0
	JSR	PC,CVF
	SOB	R3,9$
	MOV	LLSTPPTR,R2	;starting address of yellow stop
	ADD	R4,R2
	MOV	#LLINE,R0
	MOV	#7,R3
8$:	LDF	@(R2)+,AC0
	JSR	PC,CVF
	SOB	R3,8$
	EVWAIT	CSLEVT		;GRAB CONSOLE
15$:	MOV	(R5)+,R4	;start printing out the list of strings
	BEQ	2$		; if null, then done
16$:	MOV	(R4)+,R0
	BEQ	15$
	JSR	PC,PTYPSTR
	BR	16$
2$:	EVSIG	CSLEVT
	POP	<R5,R4,R3,R2>
	MOV	ALHOMP,CURYXAL	;initialize position for AL display
	RTS	PC

DATA
900$:	.WORD	PNTIT,RNSTIT,CRLFEOL,0
105$:
101$:	.WORD	BLUTIT,0
205$:
201$:	.WORD	YELTIT,0
902$:	.WORD	ARMTIT,JTTIT,CRLFEOL,HITIT,STTIT
	.WORD	ULINE,CRLFEOL,LOTIT,STTIT,LLINE,CRLFEOL,PRKTIT,0
103$:	.WORD	BPRKTIT,0
203$:	.WORD	YPRKTIT,0
904$:	.WORD	CRLFEOL,CRLFEOL,0
906$:	.WORD	JNTSTIT,CRLFEOL,ADTIT,CRLFEOL
	.WORD	EUTIT,CRLFEOL,ALTIT,RNSTIT,0
100$:	.WORD	900$,101$,902$,103$,904$,105$,906$,0
200$:	.WORD	900$,201$,902$,203$,904$,205$,906$,0
VT05DSP:.WORD	1		;1=blue arm,2=yellow arm,3=both
ULINE:	.BLKW	72.
LLINE:	.BLKW	72.
CODE
;	routine to print out joint value PRTW
DATA
UPDOK:	0			; 0 if to update the reading of arm
PRTWPD:	PDBLK	1,100,FP	; SET UP PBD FOR PRINTING ROUTINE
DSPOK:	0			; FLAG TO INDICATE IF DISPLAY WANTED
DSPOKSAV:0
NCYCLES:50.			; number of cycles before redrawing the words
CODE
PRTW:	TST	ALLIVE		;TEST IF AL IS DONE
	BNE	1$
	DISMIS			;go away, AL is done
1$:	TST	DSPOK		;Test if want the display
	BEQ	2$
	SLEEP	#1000.		;No,we dont want display
	BR	PRTW		;Go check if now we want display
2$:	TST	FRMDDT		;TEST IF JUST CAME FROM DDT
	BEQ	3$
	JSR	PC,DSPINIT	;YES, SO REINITIALIZE DISPLAY
	CLR	FRMDDT		;CLEAR FROM DDT FLAG
3$:	DEC	NCYCLES		;decrement
	BGT	4$
	MOV	#50.,NCYCLES
	JSR	PC,DSPINIT
4$:	MOV	#IOBUF,R0	;START BUILDING OUTPUT STRING IN HERE
	MOV	LTHPTR,R2	;GET POINTERS JOINT READINGS
	CMP	VT05DSP,#2
	BEQ	5$		; is yellow arm
	ADD	#BTABOFF,R2	;TO GET VALUES FOR BLUE ARM
5$:	MOV	#7,R3		;7 JOINTS IN ALL
JTCLP:	LDF 	@(R2)+,AC0	;GET JOINT ANGLE
	JSR	PC,CVF		;CONVERT JOINT ANGLE TO STRING 
	SOB	R3,JTCLP
	CLRB	(R0)+		;END OF INFO
	EVWAIT	CSLEVT
	MOV	#JTHOME,R0
	JSR	PC,PTYPSTR	;JUMP TO LINE TO PRINT CURRENT VALUES
	MOV	#IOBUF,R0
	JSR	PC,PTYPSTR	;TYPE OUT WHOLE STRING
	EVSIG	CSLEVT
	MOV	#9.,R0		;CHANGE STRING FORMAT FOR INTEGER NUMBER
	CLR	R1
	JSR	PC,FORMAT
	MOV	#IOBUF,R0	;START BUILDING OUT STRING AGAIN
	MOV	LPOTPTR,R2	;GET POINTERS TO POTS
	CMP	VT05DSP,#2	; is it yellow or blue?
	BEQ	1$		; it is yellow
	ADD	#BTABOFF,R2	;TO GET FOR BLUE ARM
1$:	MOV	#7,R3		;CONVERT THE ACTUAL POT READINGS
NEXTP:	LDCIF	@(R2),AC0	;GET A/D READING
	TST	@(R2)+		;CHECK IF ROUND UP OR DOWN
	BLT	RNDDN
	ADDF	#40000,AC0	;ROUND OFF READING
	BR	.+6
RNDDN:	SUBF	#40000,AC0
	JSR	PC,CVF		;CONVERT TO ASC
	SOB	R3,NEXTP
	JSR	PC,RSTFOR	;RESET THE FORMAT
	CLRB	(R0)		;PUT IN EMPTY BYTE
	EVWAIT	CSLEVT
	MOV	#ADHOME,R0
	JSR	PC,PTYPSTR	;JUMP TO LINE TO PRINT CURRENT VALUES
	MOV	#IOBUF,R0
	JSR	PC,PTYPSTR	;TYPE OUT WHOLE STRING
	EVSIG	CSLEVT
	MOV	#TRNADR,R0	;GET CURRENT TRANSFORM IN HERE
	MOV	LTHPTR,R1	;POINT TO THE CURRENT JOINT ANGLES
	MOV	#BLUARM,R2	;USE SOLUTION FOR BLUE ARM
	JSR	PC,@LUPDATE	;COMPUTE THE CURRENT TRANSFORM
	MOV	#TRNADR,R0	;COMPUTE THE EULER ANGLES
	MOV	#EANGLE,R1	;PUT THEM IN HERE
	JSR	PC,@LEULER
	MOV	#IOBUF,R0	;CONVERT TO STRING FOR OUTPUT
	MOV	#6,R3
	MOV	#EANGLE,R2
EANGC:	LDF	(R2)+,AC0
	JSR	PC,CVF
	SOB	R3,EANGC	;REPEAT TILL DONE
	CLRB	(R0)
	EVWAIT	CSLEVT
	MOV	#EUHOME,R0
	JSR	PC,PTYPSTR	;GET TO RIGHT PLACE
	MOV	#IOBUF,R0
	JSR	PC,PTYPSTR	;TYPE OUT THE EULER ANGLES
	EVSIG	CSLEVT
	SLEEP	#150.		;SLEEP 150 MSEC TO GIVE AL INTERPRETER A CHANCE
	JMP	PRTW
;	routine to read joint angles every 100 msec DOWHER
DATA
DWHRPD:	PDBLK	2,100,FP	; SET UP PBD FOR PROCESS THAT LOOKS AT ARM POSITION
CODE
				; EVERY 50 MSEC
DOWHER:	MOV	#BARMSB+BHANDSB,WLST	;DO A WHERE FOR THE BLUE ARM
       	MOV	#WLST,R0
	MOV	#DEVICE,R1
	TST	UPDOK
	BNE	1$
	JSR	PC,@LWHERE	;READ THE JOINT POSITIONS
	MOV	#YARMSB+YHANDSB,WLST	;DO A WHERE FOR THE YELLOW ARM
       	MOV	#WLST,R0
	MOV	#DEVICE,R1
	TST	UPDOK
	BNE	1$
	JSR	PC,@LWHERE	;READ THE JOINT POSITIONS
2$:	TST	ALLIVE		;TEST IF AL PROGRAM FINISHED
	BNE	1$
	DISMIS
1$:	SLEEP	#100.		; GO TO SLEEP FOR 100 MSECS
	TST	UPDOK
	BNE	2$		; arm is moving dont do a where
	BR	DOWHER
;LOCAL STORAGE AREA
DATA
DEVICE:	.BLKW	33	;DEVICE BLOCK FOR WHERE
IOBUF:	.BLKW	400.	;OUTPUT STRING BUFFER
EANGLE:	.BLKW	12.	;EULER ANGLES STORED HERE
WLST:	374    		;JOINT SERVO BIT
	0

;OUTPUT STRINGS
		; TOP OF VT05 SCREEN
CLRSCR:	.BYTE	HOME,CFILL,CFILL,CFILL,EOS,CFILL,CFILL,CFILL,0,0
PNTIT:	.BYTE	HOME,CFILL,CFILL,CFILL,EOL
	.ASCIZ	/	       P O I N T Y/
RNSTIT:	.ASCIZ	/   R U N T I M E   S Y S T E M/
JTTIT:	.ASCIZ	\	JT1 /X  JT2 /Y  JT3 /Z  JT4 /O  JT5 /A  JT6 /T     HAND\
HITIT:	.ASCIZ	/HIGH /
LOTIT:	.ASCIZ	/LOW  /
STTIT:	.ASCIZ	/STOPS    /
PRKTIT:	.ASCIZ	/PARK POSITION  /
BPRKTIT:.ASCIZ	/ 180.00  -90.00   14.00  -90.00   90.00    0.00/
YPRKTIT:.ASCIZ	/-180.00   90.00   14.00  -90.00   90.00   90.00/
CRPTIT:	.ASCIZ	/CURRENT POS/
ADTIT:	.ASCIZ	/A-D READINGS/
EUTIT:  .ASCIZ  /EULER ANGLES/
ALTIT:	.ASCIZ	/		   A L/
BLUTIT:	.ASCIZ	/BLUE /
YELTIT:	.ASCIZ	/YELLOW /
ARMTIT:	.ASCIZ	/ARM/
JNTSTIT:.ASCIZ	/JOINTS/
CRLFEOL:.BYTE	CR,LF,EOL,0
	.EVEN
ALHOMP:	.BYTE	ALHOMY,ALHOMX
JTHOME:	.BYTE	CAD,JTHOMY,JTHOMX,0
ADHOME:	.BYTE	CAD,ADHOMY,ADHOMX,0
EUHOME:	.BYTE	CAD,EUHOMY,EUHOMX,0
	.EVEN
CODE